import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

import serial
import time 


class Driver():
    def __init__(self):
        self.sub = rospy.Subscriber('/control_velocity_nav', Twist, self.vel_callback_nav)
	self.sub = rospy.Subscriber('/control_velocity_joy', Twist, self.vel_callback_joy)

	self.pub = rospy.Publisher('/control_velocity', Twist, queue_size=2)
	self.twist = Twist()
	self.twist_joy = Twist()

        self.twist.linear.y = 0
	self.twist.linear.z = 0
        self.twist.angular.x = 0
	self.twist.angular.y = 0

	self.count = 0
	self.joy_run = 0
	self.nav_run = 1


    def vel_callback_joy(self, msg):
        self.twist_joy.linear.x = msg.linear.x
	self.twist_joy.angular.z = msg.angular.z
	self.count = 10
	self.nav_run = 0
	if msg.linear.y == 0.123:
	    self.nav_run = 1
	print('joy control>>> callback ')

	

    def vel_callback_nav(self, msg):
	if(self.nav_run):
	    self.twist.linear.x = msg.linear.x
	    self.twist.angular.z = msg.angular.z
	    if self.count < 10:
		self.count = 10

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
    	    
            if self.count != 0:
		print ('count  %d' % self.count)
		if(self.nav_run):
			print('nav control...')
			self.count -= 1
			print('nav cmd ',self.twist.linear.x, "  ", self.twist.angular.z )
			self.pub.publish(self.twist)
		else:	
			print('joy control...')
			print('pub cmd ',self.twist_joy.linear.x, "  ", self.twist_joy.angular.z )
	    		self.pub.publish(self.twist_joy)
	    	rate.sleep()
	    else:
		print('stop ****')
		self.twist.linear.x = 0
		self.twist.angular.z = 0
		self.pub.publish(self.twist)
		rate.sleep()
		

if __name__=='__main__':
    rospy.init_node('drive_car001')
    driver = Driver()
    driver.run()





